/*
 * Communication.cpp
 *
 *  Created on: Apr 22, 2011
 *      Author: tflanzer
 */

#include "Communication.h"
#include <termios.h>
#include <unistd.h>
#include <stdlib.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <stdio.h>
#include <fcntl.h>
#include <strings.h>
#include <inttypes.h>
#include <pthread.h>
#include <math.h>
#include <stdint.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <linux/i2c-dev.h>

#define BAUDRATE_GND B115200
#define BAUDRATE_AUP B230400
#define MODEMDEVICE_GND "/dev/ttyO1"
#define MODEMDEVICE_AUP "/dev/ttyO0"
#define _POSIX_SOURCE 1

Communication::Communication(Timer &t, FileWriter *f) {

	timer = t;
	fw = f; // FileWriter
	interrupt_AUP = false;
	interrupt_GND = false;

	struct termios auptio;
	struct termios gndtio;

	// Open serial link with the autopilot
	cout << "Opening link to autopilot..." << endl;
	faup = open(MODEMDEVICE_AUP, O_RDWR | O_NOCTTY | O_NONBLOCK);
	auptio.c_cflag = BAUDRATE_AUP | CS8 | CLOCAL | CREAD;
	auptio.c_lflag = ICANON;
	cfmakeraw(&auptio);
	auptio.c_oflag &= ~OPOST;
	auptio.c_cflag &= ~PARENB;
	auptio.c_cflag &= ~CSTOPB;
	auptio.c_cflag &= ~CSIZE;
	auptio.c_cflag |= CS8;
	auptio.c_iflag &= ~(IXON | IXOFF | IXANY);
	auptio.c_cflag &= ~(CRTSCTS);
	fcntl(faup, F_SETFL, 0);

//  now clean the modem line and activate the settings for the port
	tcflush(faup, TCIFLUSH);
	tcsetattr(faup,TCSANOW,&auptio);
	cout << "Autopilot serial port opened" << endl;

	// Open serial link with the ground station
	cout << "Opening link to ground station..." << endl;
	fgnd = open(MODEMDEVICE_GND, O_RDWR | O_NOCTTY | O_NONBLOCK);
	gndtio.c_cflag = BAUDRATE_GND | CS8 | CLOCAL | CREAD;
	gndtio.c_lflag = ICANON;
	cfmakeraw(&gndtio);
	gndtio.c_oflag &= ~OPOST;
	gndtio.c_cflag &= ~PARENB;
	gndtio.c_cflag &= ~CSTOPB;
	gndtio.c_cflag &= ~CSIZE;
	gndtio.c_cflag |= CS8;
	gndtio.c_iflag &= ~(IXON | IXOFF | IXANY);
	gndtio.c_cflag &= ~(CRTSCTS);
	fcntl(fgnd, F_SETFL, 0);

//  now clean the modem line and activate the settings for the port
	tcflush(fgnd, TCIFLUSH);
	tcsetattr(fgnd,TCSANOW,&gndtio);
	cout << "Ground station serial port opened" << endl;

	// Initialize i2c
	fi2c = initializei2c();
	if (fi2c < 0) {
		cout << "Error opening i2c." << endl;
	}

//  now clean the modem line and activate the settings for the port
//	tcflush(fgnd, TCIFLUSH);
//	tcsetattr(fgnd,TCSANOW,&gndtio);
	cout << "Ground station serial port opened" << endl;

	cout << "Beginning ground station and autopilot threads" << endl;
	pthread_t threadGND;
	pthread_t threadAUP;

	int rg = pthread_create(&threadGND, NULL, &Communication::startGNDThread, this);

	int ra = pthread_create(&threadAUP, NULL, &Communication::startAUPThread, this);

	if (rg || ra){
        printf("ERROR generating serial threads in timer.cpp");
        exit(-1);
     }

    system_type = MAV_FIXED_WING;
    autopilot_type = MAV_AUTOPILOT_GENERIC;
    MAVLINK_MODE = 0;
	imuin = false;
	gpsin = false;
	servoin = false;
	tIMU[0] = 0.0; tIMU[1] = 0.01;
	tGPS[0] = 0.0; tGPS[1] = 0.10;
	datInitReceived = false;
	imuDatReceived  = false;
	runEKF = false;
	pv.killMariner = false;
	numi = 0; numg = 0;

	// Initialize latitude/longitude/altitude
	initLat = 0.0; //37.43283611;
	initLon = 0.0; //0-122.182375;
	initAlt = 0.0;
}

Communication::~Communication() {
}

void Communication::startAUPLoop(){
	while (!interrupt_AUP){
		bool flag = getAUPData();
		if (flag) {
			notifyListenersAUP();
		}
	}
}

bool Communication::getAUPData() {
	char buf[1]; // buffer to read in data
	int16_t imudat[11];
	int16_t imu2dat[6];
	float gpsdat[8];
	double mag[3], ht_abv_gnd;
	double magnitude;
	unsigned char servodat[5];
	char *buf_imuptr = (char *) imudat;
	char *buf_gpsptr = (char *) gpsdat;
	char chksum = 0;
	imuin = false;
	gpsin = false;
	servoin = false;
	chksum_flag = false;
	read(faup,buf,1);
	// Read first byte of buffer looking for message flag
	// 10 doubles I, 10 doubles, 1 char
	// 8 GPS      G, 7 doubles, 1 char
	if (buf[0] == 'I') { // IMU packet
		for (int i = 0; i < 22; i++) {
			read(faup, buf, 1);
			chksum ^= buf[0];
			*buf_imuptr = buf[0];
			buf_imuptr++;
		}
		read(faup, buf, 1); // Checksum
		if (buf[0] == chksum) {
			numi++;
			fw->updateValue("numi",numi);
			chksum_flag = true;
			imuin = true;

			// Query magnetometer/accelerometer			
			ioctl(fi2c,I2C_SLAVE,ACC_DEVICE); // Talk to accelerometer
			imu2dat[0] = read_reg_acc(fi2c, 0xA8, 2)/16;
			imu2dat[1] = read_reg_acc(fi2c, 0xAA, 2)/16;
			imu2dat[2] = read_reg_acc(fi2c, 0xAC, 2)/16;

			ioctl(fi2c,I2C_SLAVE,MAG_DEVICE); // Talk to mag
			imu2dat[3] = read_reg_mag(fi2c, 0x03, 2);
			imu2dat[4] = read_reg_mag(fi2c, 0x05, 2);
			imu2dat[5] = read_reg_mag(fi2c, 0x07, 2);
			write_reg(fi2c, 0x02, 0x01); // Start single conversion

			// Calibration constants -> convert to m/s^2
			apdat.ax1 = -0.036362*(imudat[0]-11.374621) - 0.001122*(imudat[1]-0.845866) - 0.001279*(imudat[2]+22.846367);
			apdat.ay1 = 0.000084*(imudat[0]-11.374621) + 0.037744*(imudat[1]-0.845866) - 0.000229*(imudat[2]+22.846367);
			apdat.az1 = -0.000901*(imudat[0]-11.374621) + 0.000335*(imudat[1]-0.845866) + 0.038440*(imudat[2]+22.846367);

			// Calibration constants -> convert to m/s^2
			apdat.ax2 = 0.000347*(imu2dat[0]+4.850147) + 0.036293*(imu2dat[1]+21.53314) + 0.003792*(imu2dat[2]-3.832696);
			apdat.ay2 = 0.039542*(imu2dat[0]+4.850147) - 0.000019*(imu2dat[1]+21.53314) - 0.001147*(imu2dat[2]-3.832696);
			apdat.az2 = 0.000134*(imu2dat[0]+4.850147) - 0.000916*(imu2dat[1]+21.53314) + 0.039375*(imu2dat[2]-3.832696);
			
			// Convert to rad/s
			apdat.p = imudat[3]*-0.0012141421;
			apdat.q = imudat[4]*0.0012141421;
			apdat.r = imudat[5]*-0.0012141421;

			// Calibrate and convert to unit vector in body frame
			mag[0] = -0.004415*(imu2dat[3]+159.588453) - 0.090094*(imu2dat[4]-35.558917) - 0.007732*(imu2dat[5]+155.844219); 	// uT
			mag[1] = -0.090531*(imu2dat[3]+159.588453) + 0.002138*(imu2dat[4]-35.558917) + 0.002098*(imu2dat[5]+155.844219);	// uT
			mag[2] =  -0.000619*(imu2dat[3]+159.588453) + 0.006075*(imu2dat[4]-35.558917) - 0.111448*(imu2dat[5]+155.844219);	// uT
			fw->updateValue("rawmagx",mag[0]);
			fw->updateValue("rawmagy",mag[1]);
			fw->updateValue("rawmagz",mag[2]);
			magnitude = 1.0/sqrt(mag[0]*mag[0]+mag[1]*mag[1]+mag[2]*mag[2]);
			apdat.mx = mag[0]*magnitude;
			apdat.my = mag[1]*magnitude;
			apdat.mz = mag[2]*magnitude;
			
			if (!imuDatReceived) {
				pv.diffb = (double)(imudat[6] - 2048);
				imuDatReceived = true;
			}

			// Convert other measurements
			apdat.diffpress = fabs((imudat[6]-pv.diffb)*1.2207031 - 2500.0);	// Convert to Pa -> must be positive
			apdat.baropress = imudat[7]+100000.0; 					// Convert to Pa
			apdat.barotemp = imudat[8]/10.0;						// Convert to C
			apdat.sonar_range = imudat[9]*0.003175775335775; // Sonar range in meters
			apdat.autoFlag = imudat[10]; // Autopilot flag

			tIMU[0] = tIMU[1];
			tIMU[1] = timer.getTime();
			if (tIMU[1] < tIMU[0]) {
				chksum_flag = false;
			}

			// Update IMU packets for output file
			updateIMUFile();
		}
	} else if (buf[0] == 'G') { // GPS packet
		for (int i = 0; i < 32; i++) {
			read(faup, buf, 1);
			chksum ^= buf[0];
			*buf_gpsptr = buf[0];
			buf_gpsptr++;
		}
		read(faup, buf, 1); // Checksum
		if (buf[0] == chksum) {
			numg++;
			fw->updateValue("numg",numg);
			chksum_flag = true;
			gpsin = true;
			apdat.GPStime = (double) gpsdat[0];
			apdat.lat = (double) gpsdat[1];
			apdat.lon = (double) gpsdat[2];
			if ((!datInitReceived) && (apdat.lat!=0.0) && (apdat.lon!=0.0) &&
					(apdat.lat <= 90)  && (apdat.lat >= -90.0) &&
					(apdat.lon <= 180) && (apdat.lon >= -180.0) && (numg > 40)) {
				lon2meter = LAT2METER*cos(apdat.lat*M_PI/180.0);
				initLat = apdat.lat;
				initLon = apdat.lon;
				initAlt = apdat.GPS_D;
				pv.initLat = initLat;
				pv.initLon = initLon;
				datInitReceived = true;
			}

			if ((apdat.sonar_range < 7) && (apdat.sonar_range > 0.5)) {
				ht_abv_gnd = (apdat.sonar_range+1.75/2.0*sin(sv.phi))/(cos(sv.phi)*cos(sv.theta));
				initAlt = apdat.GPS_D + ht_abv_gnd;
			}

			apdat.GPS_N = LAT2METER*(apdat.lat-initLat);
			apdat.GPS_E = lon2meter*(apdat.lon-initLon);
			apdat.GPS_D = (double) gpsdat[3] - initAlt;
			apdat.GPS_VN = (double) gpsdat[4];
			apdat.GPS_VE = (double) gpsdat[5];
			apdat.GPS_VD = (double) gpsdat[6];

			apdat.numSats = (double) (floor(gpsdat[7]/100.0));
			apdat.fix = (double) ((int)gpsdat[7]%100);

			printf("Dp = %f, iAlt = %f, hab = %f, GPSD = %f\n",sv.Dp,initAlt,ht_abv_gnd,
					(double)gpsdat[3]);

			if (apdat.fix == 0) {
				chksum_flag = false;
			}

			tGPS[0] = tGPS[1];
			tGPS[1] = timer.getTime();

			// Update GPS info for output file
			updateGPSFile();
		}
	}
	return chksum_flag;
}

void Communication::startGNDLoop(){
	while (!interrupt_GND){
		bool flag = getGNDData();
		if (flag) {
			notifyListenersGND();
			flag = false;
		}
	}
}

bool Communication::getGNDData() {
	bool flag = false;
	mavlink_message_t msg;
	mavlink_status_t status;
	mavlink_message_t msgdwn;
	uint8_t buf[1]; // buffer to read in data
	uint8_t bufdwn[MAVLINK_MAX_PACKET_LEN]; // buffer to read in data
	read(fgnd,buf,1);

	if(mavlink_parse_char(MAVLINK_COMM_0,*buf,&msg,&status)) {
		switch (msg.msgid) {

		// Mode command
		case MAVLINK_MSG_ID_SET_MODE: {
			gndmsgtype = MAVLINK_MSG_ID_SET_MODE;
			MAVLINK_MODE = mavlink_msg_set_mode_get_mode(&msg);
			cout << "Mode = " << (int)MAVLINK_MODE << endl;
			flag = true;
			if (MAVLINK_MODE == 7) {
				pv.killMariner = true;
				flag = false;
			}
			break;
		}

		// Read single parameter
		case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
			gndmsgtype = MAVLINK_MSG_ID_PARAM_REQUEST_READ;
			cout << "request read" << endl;
			break;
		}

		// Send parameters to ground station
		case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
			gndmsgtype = MAVLINK_MSG_ID_PARAM_REQUEST_READ;
			returnParams();
			break;
		}

		// Write updated parameters from ground station
		case MAVLINK_MSG_ID_PARAM_SET: {
			gndmsgtype = MAVLINK_MSG_ID_PARAM_SET;

			uint8_t bufdwn[MAVLINK_MAX_PACKET_LEN];
			mavlink_param_set_t param_set;

			// Decode parameter message
			mavlink_msg_param_set_decode(&msg, &param_set);

			// Set value in control gains structure
			ctrlGns.cmap[(char*)param_set.param_id] = param_set.param_value;

			// Send gain to ground station to confirm receipt
			mavlink_msg_param_value_pack(100,200,&msg,param_set.param_id,
					(float)ctrlGns.cmap[(char*)param_set.param_id],
					(uint16_t)ctrlGns.numgains, (uint16_t)0);
			uint16_t len = mavlink_msg_to_send_buffer(bufdwn, &msg);
			int n = write(fgnd,bufdwn,len);

			cout << "Sending confirmation that " << (char*)param_set.param_id << " = " <<
					ctrlGns.cmap[(char*)param_set.param_id] <<
					" onboard." << endl;

			flag = true;
			break;
		}

		// GCS is requesting to send up a list of waypoints
		case MAVLINK_MSG_ID_WAYPOINT_COUNT: {
			gndmsgtype = MAVLINK_MSG_ID_WAYPOINT_COUNT;

			// Record number of waypoints from list
			cout << "Command list being sent.." << endl;
			pv.numwps = (int)mavlink_msg_waypoint_count_get_count(&msg);
			cout << "Total number of waypoints = " << pv.numwps << endl;
			// Send first waypoint request to GCS, starting w/seq #0
			mavlink_msg_waypoint_request_pack(100,1,&msg,255,0,
						(uint16_t)0);
			uint16_t len = mavlink_msg_to_send_buffer(bufdwn, &msg);
			write(fgnd,bufdwn,len);
			break;
		}
		case MAVLINK_MSG_ID_WAYPOINT: {
			gndmsgtype = MAVLINK_MSG_ID_WAYPOINT;
			// GCS is sending waypoint information
			unpackWaypointInfo(&msg);
			break;
		}
		case MAVLINK_MSG_ID_WAYPOINT_CURRENT: {
			gndmsgtype = MAVLINK_MSG_ID_WAYPOINT_CURRENT;
			// GCS is setting current waypoint
			pv.wpidx[1] = (int)mavlink_msg_waypoint_current_get_seq(&msg);
			// Send to GCS message with the updated new waypoint #
			mavlink_msg_waypoint_current_pack(100,1,&msgdwn,(uint16_t)pv.wpidx[1]);
			uint16_t len = mavlink_msg_to_send_buffer(bufdwn, &msgdwn);
			write(fgnd,bufdwn,len);
			break;
		}
		// QGC is requesting a list of current waypoints
		case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: {
			cout << "QGC requesting waypoint list." << endl;
			// Reply telling QGC how many waypoints there are
			mavlink_msg_waypoint_count_pack(100,1,&msgdwn,255,0,(uint16_t)pv.numwps);
			uint16_t len = mavlink_msg_to_send_buffer(bufdwn, &msgdwn);
			write(fgnd,bufdwn,len);
			cout << "Total number of waypoints = " << pv.numwps << "." << endl;
			// QGC will now request waypoints in order using
			// MAVLINK_MSG_ID_WAYPOINT_REQUEST message
		}

		// QGC is requesting individual waypoint information
		case MAVLINK_MSG_ID_WAYPOINT_REQUEST: {
			mavlink_waypoint_request_t wp_request;
			// Determine waypoint index being requested
			mavlink_msg_waypoint_request_decode(&msg,&wp_request);
			// Return waypoint latitude and longitude
			float waypointLat, waypointLon;
			uint8_t current = 0;
			if (wp_request.seq == (uint8_t)pv.wpidx[1]) current = 1;
			waypointLat = pv.waypointNorth[wp_request.seq]/LAT2METER + initLat;
			waypointLon = pv.waypointEast[wp_request.seq]/lon2meter  + initLon;
			mavlink_msg_waypoint_pack(100,1,&msgdwn,255,0,wp_request.seq,
					0, 0, (uint8_t) current, 0, 0, 0, 0, 0, waypointLat,
					waypointLon, -sv.Dp);
			uint16_t len = mavlink_msg_to_send_buffer(bufdwn, &msgdwn);
			write(fgnd,bufdwn,len);
		}
		default:
			break;
		}
	}
	return flag;
}

void Communication::returnParams() {
	mavlink_message_t msg;
	uint8_t bufdwn[MAVLINK_MAX_PACKET_LEN]; // buffer to read in data
	for (int i = 0; i < ctrlGns.numgains; i++) {
		mavlink_msg_param_value_pack(100,200,&msg,(int8_t*)ctrlGns.gainName[i],
				(float)ctrlGns.cmap[ctrlGns.gainName[i]],
				(uint16_t)4, (uint16_t)i);
		uint16_t len = mavlink_msg_to_send_buffer(bufdwn, &msg);
		int n = write(fgnd,bufdwn,len);
		cout << "Sent " << ctrlGns.gainName[i] << " = " <<
				ctrlGns.cmap[ctrlGns.gainName[i]] << " to QGC." << endl;
	}
	return;
}

void Communication::fiveHzUpdate(Timer &timer) {
	mavlink_message_t msg;
	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
	uint16_t len;
	int n;

	// GPS Packet
	uint64_t usec = (uint64_t)(1.0e6*timer.getTime());
	uint8_t fix_type = 0;
	if (!datInitReceived) {
		fix_type = 0;
	} else {
		fix_type = 3;
	}

    if (datInitReceived) {
		mavlink_msg_global_position_pack(100,200,&msg,usec, (float) apdat.lat,
				(float) apdat.lon, (float) -sv.Dp, (float) apdat.GPS_VN,
				(float) apdat.GPS_VE, (float) apdat.fix);
		len = mavlink_msg_to_send_buffer(buf, &msg);
		n = write(fgnd,buf,len);

		// Attitude packet
		mavlink_msg_attitude_pack(100,200,&msg,usec, (float) sv.phi, (float) sv.theta,
				(float) sv.psi, (float) apdat.p, (float) apdat.q, (float) apdat.r);
		len = mavlink_msg_to_send_buffer(buf, &msg);
		n = write(fgnd,buf,len);

		// VFR HUD
		float airspeed = sqrt(2*apdat.diffpress/1.225);
		float gndspeed = sqrt(apdat.GPS_VN*apdat.GPS_VN+apdat.GPS_VE*apdat.GPS_VE);
		float hdg = atan2(apdat.GPS_VE,apdat.GPS_VN);
		mavlink_msg_vfr_hud_pack(100,200,&msg, airspeed, gndspeed, (int16_t)hdg,
				(uint16_t) (100*cv.thrPercent), (float)-sv.Dp, (float) -sv.VDi);
		len = mavlink_msg_to_send_buffer(buf, &msg);
		n = write(fgnd,buf,len);
    }
}

// Send heartbeat to ground station at one Hz
void Communication::oneHzUpdate(Timer &timer){
	mavlink_message_t msg;
	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
	mavlink_msg_heartbeat_pack(100, 200, &msg, system_type, autopilot_type);
	uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
	int n = write(fgnd,buf,len);
	if (n < 0) {
		cout << "Heartbeat failed to send at " << timer.getTime() << endl;
	} else {
		cout << "Heartbeat sent at " << timer.getTime() << endl;
	}
}

void Communication::unpackWaypointInfo(const mavlink_message_t* msg) {

	// Decode message into waypoint structure
	mavlink_waypoint_t waypoint;
	mavlink_message_t msgdwn;
	uint8_t bufdwn[MAVLINK_MAX_PACKET_LEN]; // buffer to send data
	mavlink_msg_waypoint_decode(msg, &waypoint);

	// Record data into global waypoint vector
	int wpcmd = (int)waypoint.command; // 16 is standard waypoint msg
	int wpidx = (int)waypoint.seq; // Waypoint number
	int frame = (int)waypoint.frame; // Coordinate frame
	// 0 = abs alt/global (global = lat/lon)
	// 1 = abs alt/local  (local = N/E/-D)
	// 2 = Mission (non-std message)
	// 3 = rel alt/global

	if (wpcmd == MAV_CMD_NAV_WAYPOINT) { //
		if (frame == 0) { // Coordinates specified in lat/lon
			waypointLatDeg[wpidx]   = (double)waypoint.x;
			waypointLonDeg[wpidx]   = (double)waypoint.y;
			pv.waypointAlt[wpidx]   = (double)waypoint.z;
			// Its important that initLat/initLon have been defined -
			// do not send waypoint list until GPS lock has been established
			pv.waypointNorth[wpidx] = LAT2METER*(waypointLatDeg[wpidx]-initLat);
			pv.waypointEast[wpidx]  = lon2meter*(waypointLonDeg[wpidx]-initLon);
		} else if (frame == 1) { // Coordinates in relative frame
			pv.waypointNorth[wpidx] = (double)waypoint.x;
			pv.waypointEast[wpidx]  = (double)waypoint.y;
			pv.waypointAlt[wpidx]   = (double)waypoint.z;

		}
		cout << "Waypoint # " << wpidx << " received. " <<
				"N = " << pv.waypointNorth[wpidx] <<
				" E = " << pv.waypointEast[wpidx] <<
				" Alt = " << pv.waypointAlt[wpidx] << endl;
	} else if (wpcmd == MAV_CMD_DO_CHANGE_SPEED) {
		// Controller only works for airspeed (param1 = 0)
		if ((int)waypoint.param1==0) { // (param1 = 1 indicates gnd speed)
			pv.Vtarget = (double)waypoint.param2;
			cout << "Airspeed set to " << pv.Vtarget << endl;
		}
	}

	if (wpidx == pv.numwps-1) {
		// If the waypoint index is the last in the sequence then
		// send notification to GCS that data has been received
		mavlink_msg_waypoint_ack_pack(100,1,&msgdwn,255,0,(uint8_t) 0);
		uint16_t len = mavlink_msg_to_send_buffer(bufdwn, &msgdwn);
		write(fgnd,bufdwn,len);
		cout << "All waypoints received. Total # of waypoints = " <<
			pv.numwps << endl;
	} else { // Have not received all the waypoint info, request the next one
		mavlink_msg_waypoint_request_pack(100,1,&msgdwn,255,0,
					(uint16_t)(wpidx+1));
		uint16_t len = mavlink_msg_to_send_buffer(bufdwn, &msgdwn);
		write(fgnd,bufdwn,len);
		cout << "Requesting data for waypoint # " << (wpidx+1) << endl;
	}
}

void *Communication::startAUPThread(void *ptr){
	Communication *comptr = (Communication *)ptr;
	comptr->startAUPLoop();
	pthread_exit(NULL);
}
void *Communication::startGNDThread(void *ptr){
	Communication *comptr = (Communication *)ptr;
	comptr->startGNDLoop();
	pthread_exit(NULL);
}
void Communication::notifyListenersAUP(){
	list<AUPListener*>::iterator it;
	for(it = listenersAUP.begin(); it != listenersAUP.end(); ++it){
		(*it)->AUPdataReceived(*this);
	}
}
void Communication::notifyListenersGND(){
	list<GNDListener*>::iterator it;
	for(it = listenersGND.begin(); it != listenersGND.end(); ++it){
		(*it)->GNDdataReceived(*this);
	}
}
void Communication::addListenerAUP(AUPListener *listener){
	listenersAUP.push_back(listener);
}
void Communication::addListenerGND(GNDListener *listener){
	listenersGND.push_back(listener);
}
void Communication::removeListenerAUP(AUPListener *listener){
	listenersAUP.remove(listener);
}
void Communication::removeListenerGND(GNDListener *listener){
	listenersGND.remove(listener);
}
void Communication::closeSerialPorts() {
	close(faup);
	close(fgnd);
}

int Communication::initializei2c() {

	int fh = open("/dev/i2c-3",O_RDWR);

	// Write accelerometer configurations
	ioctl(fh,I2C_SLAVE,ACC_DEVICE);

	write_reg(fh, 0x23, 0x30); // Setting +/- 8 g range
	write_reg(fh, 0x20, 0x2F); // Start sampling at 100Hz

	// Write magnetometer configurations
	ioctl(fh,I2C_SLAVE,MAG_DEVICE);

	write_reg(fh, 0x00, 0x18); // Set to 75 Hz data output rate
	write_reg(fh, 0x01, 0x20); // Set gain to +/- 1.3 gauss
	write_reg(fh, 0x02, 0x01); // Start continuous conversion

	return fh;
}

int16_t Communication::read_reg_acc(int fh, uint8_t reg, int count){
	uint8_t data[2];
	if (count < 1 || count > 2)
		return -1;

	data[0] = reg;
	if (write(fh, &data, 1) != 1) {
		perror("write before read");
		return -1;
	}

	data[1] = 0;

	if (read(fh, &data, count) != count) {
		perror("read");
		return -1;
	}

	return (data[1] << 8) + data[0];

}

int16_t Communication::read_reg_mag(int fh, uint8_t reg, int count){
	uint8_t data[2];
	if (count < 1 || count > 2)
		return -1;

	data[0] = reg;
	if (write(fh, &data, 1) != 1) {
		perror("write before read");
		return -1;
	}

	data[1] = 0;

	if (read(fh, &data, count) != count) {
		perror("read");
		return -1;
	}

	return (data[0] << 8) + data[1];

}

/* Only doing 8 bit writes here. The device allows multi-byte writes.
 * See the manual.*/

int Communication::write_reg(int fh, uint8_t reg, uint8_t val){
	uint8_t data[2];
	data[0] = reg;
	data[1] = val;

	if (write(fh, &data, 2) != 2) {
		perror("write");
		return -1;	}
	return 1;
}

void Communication::updateIMUFile() {
	fw->updateValue("baropress",apdat.baropress);
	fw->updateValue("diffpress",apdat.diffpress);
	fw->updateValue("sonar_range",apdat.sonar_range);
	fw->updateValue("ax1",apdat.ax1);
	fw->updateValue("ay1",apdat.ay1);
	fw->updateValue("az1",apdat.az1);
	fw->updateValue("ax2",apdat.ax2);
	fw->updateValue("ay2",apdat.ay2);
	fw->updateValue("az2",apdat.az2);
	fw->updateValue("p",apdat.p);
	fw->updateValue("q",apdat.q);
	fw->updateValue("r",apdat.r);
	fw->updateValue("mx",apdat.mx);
	fw->updateValue("my",apdat.my);
	fw->updateValue("mz",apdat.mz);
	fw->updateValue("barotemp",apdat.barotemp);
	fw->updateValue("autoFlag",apdat.autoFlag);

	return;
}

void Communication::updateGPSFile() {
	fw->updateValue("GPS_N",apdat.GPS_N);
	fw->updateValue("GPS_E",apdat.GPS_E);
	fw->updateValue("GPS_D",apdat.GPS_D);
	fw->updateValue("GPS_VN",apdat.GPS_VN);
	fw->updateValue("GPS_VE",apdat.GPS_VE);
	fw->updateValue("GPS_VD",apdat.GPS_VD);
	fw->updateValue("GPStime",apdat.GPStime);
	fw->updateValue("numSats",apdat.numSats);
	fw->updateValue("fix",apdat.fix);

	return;
}
